function penalty = judge_constraint(t,d_min)
%JUDGE_CONSTRAINT 判断是否符合距离约束

    % 惩罚项，确保最小距离约束
    N = size(t,2);
    penalty = 0;
    for i = 1:N
        for j = i+1:N
            if norm(t(:,i) - t(:,j)) < d_min
                penalty = penalty + 1;
            end
        end
    end
    if penalty>0
        disp('不符合距离约束')
    end
end

